/*
 * Copyright 2016 The Cartographer Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d.h"

#include <algorithm>
#include <cmath>
#include <deque>
#include <functional>
#include <limits>

#include "Eigen/Geometry"
#include "absl/memory/memory.h"
#include "cartographer/common/math.h"
#include "cartographer/mapping/2d/grid_2d.h"
#include "cartographer/sensor/point_cloud.h"
#include "cartographer/transform/transform.h"
#include "glog/logging.h"

namespace cartographer
{
    namespace mapping
    {
        namespace scan_matching
        {
            namespace
            {

                // A collection of values which can be added and later removed, and the maximum
                // of the current values in the collection can be retrieved.
                // All of it in (amortized) O(1).
                class SlidingWindowMaximum
                {
                public:
                    void AddValue(const float value)
                    {
                        while (!non_ascending_maxima_.empty() && value > non_ascending_maxima_.back())
                        {
                            non_ascending_maxima_.pop_back();
                        }
                        non_ascending_maxima_.push_back(value);
                    }

                    void RemoveValue(const float value)
                    {
                        // DCHECK for performance, since this is done for every value in the
                        // precomputation grid.
                        DCHECK(!non_ascending_maxima_.empty());
                        DCHECK_LE(value, non_ascending_maxima_.front());
                        if (value == non_ascending_maxima_.front())
                        {
                            non_ascending_maxima_.pop_front();
                        }
                    }

                    float GetMaximum() const
                    {
                        // DCHECK for performance, since this is done for every value in the
                        // precomputation grid.
                        DCHECK_GT(non_ascending_maxima_.size(), 0);
                        return non_ascending_maxima_.front();
                    }

                    void CheckIsEmpty() const
                    {
                        CHECK_EQ(non_ascending_maxima_.size(), 0);
                    }

                private:
                    // Maximum of the current sliding window at the front. Then the maximum of the
                    // remaining window that came after this values first occurrence, and so on.
                    std::deque<float> non_ascending_maxima_;
                };

            } // namespace

            proto::FastCorrelativeScanMatcherOptions2D CreateFastCorrelativeScanMatcherOptions2D(
                common::LuaParameterDictionary *const parameter_dictionary)
            {
                proto::FastCorrelativeScanMatcherOptions2D options;
                options.set_linear_search_window(parameter_dictionary->GetDouble("linear_search_window"));
                options.set_angular_search_window(parameter_dictionary->GetDouble("angular_search_window"));
                options.set_branch_and_bound_depth(parameter_dictionary->GetInt("branch_and_bound_depth"));
                return options;
            }

            // https://blog.csdn.net/Jeff_zjf/article/details/133210223
            // 滑窗取最大概率作为当前分辨率格子的概率
            PrecomputationGrid2D::PrecomputationGrid2D(const Grid2D &grid, const CellLimits &limits, const int width,
                                                       std::vector<float> *reusable_intermediate_grid)
                : offset_(-width + 1, -width + 1)
                , wide_limits_(limits.num_x_cells + width - 1, limits.num_y_cells + width - 1)
                , min_score_(1.f - grid.GetMaxCorrespondenceCost())
                , max_score_(1.f - grid.GetMinCorrespondenceCost())
                , cells_(wide_limits_.num_x_cells * wide_limits_.num_y_cells)
            {
                CHECK_GE(width, 1);
                CHECK_GE(limits.num_x_cells, 1);
                CHECK_GE(limits.num_y_cells, 1);
                const int stride = wide_limits_.num_x_cells;
                // First we compute the maximum probability for each (x0, y) achieved in the
                // span defined by x0 <= x < x0 + width.
                std::vector<float> &intermediate = *reusable_intermediate_grid;
                intermediate.resize(wide_limits_.num_x_cells * limits.num_y_cells);
                for (int y = 0; y != limits.num_y_cells; ++y)
                {
                    SlidingWindowMaximum current_values;
                    current_values.AddValue(1.f - std::abs(grid.GetCorrespondenceCost(Eigen::Array2i(0, y))));
                    for (int x = -width + 1; x != 0; ++x)
                    {
                        intermediate[x + width - 1 + y * stride] = current_values.GetMaximum();
                        if (x + width < limits.num_x_cells)
                        {
                            current_values.AddValue(1.f - std::abs(grid.GetCorrespondenceCost(Eigen::Array2i(x + width, y))));
                        }
                    }
                    for (int x = 0; x < limits.num_x_cells - width; ++x)
                    {
                        intermediate[x + width - 1 + y * stride] = current_values.GetMaximum();
                        current_values.RemoveValue(1.f - std::abs(grid.GetCorrespondenceCost(Eigen::Array2i(x, y))));
                        current_values.AddValue(1.f - std::abs(grid.GetCorrespondenceCost(Eigen::Array2i(x + width, y))));
                    }
                    for (int x = std::max(limits.num_x_cells - width, 0); x != limits.num_x_cells; ++x)
                    {
                        intermediate[x + width - 1 + y * stride] = current_values.GetMaximum();
                        current_values.RemoveValue(1.f - std::abs(grid.GetCorrespondenceCost(Eigen::Array2i(x, y))));
                    }
                    current_values.CheckIsEmpty();
                }
                // For each (x, y), we compute the maximum probability in the width x width
                // region starting at each (x, y) and precompute the resulting bound on the
                // score.
                for (int x = 0; x != wide_limits_.num_x_cells; ++x)
                {
                    SlidingWindowMaximum current_values;
                    current_values.AddValue(intermediate[x]);
                    for (int y = -width + 1; y != 0; ++y)
                    {
                        cells_[x + (y + width - 1) * stride] = ComputeCellValue(current_values.GetMaximum());
                        if (y + width < limits.num_y_cells)
                        {
                            current_values.AddValue(intermediate[x + (y + width) * stride]);
                        }
                    }
                    for (int y = 0; y < limits.num_y_cells - width; ++y)
                    {
                        cells_[x + (y + width - 1) * stride] = ComputeCellValue(current_values.GetMaximum());
                        current_values.RemoveValue(intermediate[x + y * stride]);
                        current_values.AddValue(intermediate[x + (y + width) * stride]);
                    }
                    for (int y = std::max(limits.num_y_cells - width, 0); y != limits.num_y_cells; ++y)
                    {
                        cells_[x + (y + width - 1) * stride] = ComputeCellValue(current_values.GetMaximum());
                        current_values.RemoveValue(intermediate[x + y * stride]);
                    }
                    current_values.CheckIsEmpty();
                }
            }

            uint8 PrecomputationGrid2D::ComputeCellValue(const float probability) const
            {
                const int cell_value = common::RoundToInt((probability - min_score_) * (255.f / (max_score_ - min_score_)));
                CHECK_GE(cell_value, 0);
                CHECK_LE(cell_value, 255);
                return cell_value;
            }

            PrecomputationGridStack2D::PrecomputationGridStack2D(const Grid2D &grid,
                                                                 const proto::FastCorrelativeScanMatcherOptions2D &options)
            {
                CHECK_GE(options.branch_and_bound_depth(), 1);
                const int max_width = 1 << (options.branch_and_bound_depth() - 1);
                precomputation_grids_.reserve(options.branch_and_bound_depth());
                std::vector<float> reusable_intermediate_grid;
                const CellLimits limits = grid.limits().cell_limits();
                reusable_intermediate_grid.reserve((limits.num_x_cells + max_width - 1) * limits.num_y_cells);
                // i=0时表示原始的分辨率，i=6时表示64个格子合并成一个(取最大概率，实际并没有合并格子)
                for (int i = 0; i != options.branch_and_bound_depth(); ++i)
                {
                    const int width = 1 << i;
                    precomputation_grids_.emplace_back(grid, limits, width, &reusable_intermediate_grid);
                }
            }

            FastCorrelativeScanMatcher2D::FastCorrelativeScanMatcher2D(const Grid2D &grid,
                                                                       const proto::FastCorrelativeScanMatcherOptions2D &options)
                : options_(options)
                , limits_(grid.limits())
                , precomputation_grid_stack_(absl::make_unique<PrecomputationGridStack2D>(grid, options))
            {
            }

            FastCorrelativeScanMatcher2D::~FastCorrelativeScanMatcher2D()
            {
            }

            bool FastCorrelativeScanMatcher2D::Match(const transform::Rigid2d &initial_pose_estimate, const sensor::PointCloud &point_cloud,
                                                     const float min_score, float *score, transform::Rigid2d *pose_estimate,
                                                     int *check_code) const
            {
                const SearchParameters search_parameters(options_.linear_search_window(), options_.angular_search_window(), point_cloud,
                                                         limits_.resolution());
                // 以给定初始位姿开始搜索
                return MatchWithSearchParameters(search_parameters, initial_pose_estimate, point_cloud, min_score, score, pose_estimate,
                                                 check_code);
            }

            bool FastCorrelativeScanMatcher2D::MatchFullSubmap(const sensor::PointCloud &point_cloud, float min_score, float *score,
                                                               transform::Rigid2d *pose_estimate, int *check_code) const
            {
                // Compute a search window around the center of the submap that includes it fully.
                const SearchParameters search_parameters(1e6 * limits_.resolution(), // Linear search window, 1e6 cells/direction.
                                                         M_PI, // Angular search window, 180 degrees in both directions.
                                                         point_cloud, limits_.resolution());
                const transform::Rigid2d center = transform::Rigid2d::Translation(
                    limits_.max() -
                    0.5 * limits_.resolution() * Eigen::Vector2d(limits_.cell_limits().num_y_cells, limits_.cell_limits().num_x_cells));
                // 以栅格地图的中心为初始位姿开始搜索
                return MatchWithSearchParameters(search_parameters, center, point_cloud, min_score, score, pose_estimate, check_code);
            }

            /// @brief 最终的匹配函数,所有的东西都是通过这个函数来进行匹配的;
            /// @param search_parameters
            /// @param initial_pose_estimate  初始的位姿
            /// @param point_cloud
            /// @param min_score 新位姿分数必须大于min_score
            /// @param score 最优位姿得分
            /// @param pose_estimate 最优位姿
            /// @return
            bool FastCorrelativeScanMatcher2D::MatchWithSearchParameters(SearchParameters search_parameters,
                                                                         const transform::Rigid2d &initial_pose_estimate,
                                                                         const sensor::PointCloud &point_cloud, float min_score,
                                                                         float *score, transform::Rigid2d *pose_estimate,
                                                                         int *check_code) const
            {
                CHECK(score != nullptr);
                CHECK(pose_estimate != nullptr);
                const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.rotation();
                // 先旋转点云，使得点云坐标系与初始位姿的姿态对齐，主要为了后面使用采样的yaw来旋转点云
                auto rotation_angle = Eigen::AngleAxisf(initial_rotation.cast<float>().angle(), Eigen::Vector3f::UnitZ());
                auto rotation_rigid = transform::Rigid3f::Rotation(rotation_angle);
                const sensor::PointCloud rotated_point_cloud = sensor::TransformPointCloud(point_cloud, rotation_rigid);
                // 采样若干yaw，将当前点云按照每个yaw旋转，得到若干帧旋转后的点云
                const std::vector<sensor::PointCloud> rotated_scans = GenerateRotatedScans(rotated_point_cloud, search_parameters);
                // 计算每一帧每个点对应的栅格索引，点的索引存储在discrete_scans中;
                // discrete_scans与rotated_scans大小相同，discrete_scans仅存点的栅格索引，点数据存在rotated_scans中
                auto translation = Eigen::Translation2f(initial_pose_estimate.translation().x(), initial_pose_estimate.translation().y());
                const std::vector<DiscreteScan2D> discrete_scans = DiscretizeScans(limits_, rotated_scans, translation);
                // 遍历所有点的栅格索引，确定最小和最大索引边界，尽量减小搜索范围
                search_parameters.ShrinkToFit(discrete_scans, limits_.cell_limits());
                // 1) 在最低分辨率地图中生成若干候选位姿
                // 2) 拿每个候选位姿与所有采样点云帧(栅格索引)到地图中去累加概率作为得分
                const std::vector<Candidate2D> lowest_resolution_candidates =
                    ComputeLowestResolutionCandidates(discrete_scans, search_parameters);
                // 分支定界搜索，分辨率从低到高
                const Candidate2D best_candidate = BranchAndBound(discrete_scans, search_parameters, lowest_resolution_candidates,
                                                                  precomputation_grid_stack_->max_depth(), min_score);
                // 检测最优解的值，如果大于指定阈值min_score就认为匹配成功
                if (best_candidate.score > min_score)
                {
                    int check_code_tmp = CheckBestCandidate(precomputation_grid_stack_->Get(best_candidate.resolution_depth),
                                                            discrete_scans, best_candidate);
                    LOG(INFO) << "check_code1 " << check_code_tmp;
                    if (check_code != NULL && check_code)
                        *check_code = check_code_tmp;
                    auto &options = ClosureCheckOption::GetInstance();
                    if (options.closure_check_active && check_code_tmp < 1)
                        return false;
                    *score = best_candidate.score;
                    *pose_estimate = transform::Rigid2d({initial_pose_estimate.translation().x() + best_candidate.x,
                                                         initial_pose_estimate.translation().y() + best_candidate.y},
                                                        initial_rotation * Eigen::Rotation2Dd(best_candidate.orientation));
                    return true;
                }
                return false;
            }

            int FastCorrelativeScanMatcher2D::CheckBestCandidate(const PrecomputationGrid2D &precomputation_grid,
                                                                 const std::vector<DiscreteScan2D> &discrete_scans,
                                                                 const Candidate2D &best_candidate) const
            {
                auto &options = ClosureCheckOption::GetInstance();
                float histogram_resolution = options.closure_check_histogram_resolution;
                int group_size = 360.0f / histogram_resolution;
                std::vector<int> histogram;
                histogram.resize(group_size, 0);
                int cloud_size = 0;
                int high_score_size = 0;
                // 根据最佳候选所在帧索引取对应的栅格索引帧
                for (const Eigen::Array2i &xy_index : discrete_scans[best_candidate.scan_index])
                {
                    // xy_index是旋转帧里的索引(没有位姿的平移量)，这里加上位姿的偏移量就是点在栅格中的索引
                    const Eigen::Array2i proposed_xy_index(xy_index.x() + best_candidate.x_index_offset,
                                                           xy_index.y() + best_candidate.y_index_offset);
                    Eigen::Array2f index_new; // 计算经过中心偏移的索引
                    if (!precomputation_grid.GetCenterOffSetIndex(proposed_xy_index, index_new))
                        continue;
                    cloud_size++;
                    int score = precomputation_grid.GetValue(proposed_xy_index);
                    if (score < options.closure_check_socre_min)
                        continue; // 跳过得分太低的点
                    high_score_size++;
                    float angle = std::atan2(index_new.y(), index_new.x());
                    angle += M_PI;
                    angle = angle * (180.0 / M_PI);
                    int group_id = angle / histogram_resolution;
                    if (group_id >= group_size)
                        group_id = group_size - 1;
                    histogram[group_id]++;
                }
                if (cloud_size < 16)
                    return -1;
                if (high_score_size < 8)
                    return -2;
                int valid_size = 0;
                int valid_group_size = 0;
                int uncontinuous_num = -1;
                size_t last_continuous_idx = -1;
                for (size_t i = 0; i < histogram.size(); i++)
                {
                    if (histogram[i] < options.closure_check_histogram_min_num)
                        continue; // 计数太小
                    if (uncontinuous_num < 0)
                    {
                        uncontinuous_num = 1;
                    }
                    else if ((last_continuous_idx + 1) != i)
                    {
                        uncontinuous_num++;
                    }
                    last_continuous_idx = i;
                    valid_group_size++;
                    valid_size += histogram[i];
                }
                LOG(INFO) << "CheckBestCandidate valid_size " << valid_size << " cloud_size " << cloud_size << " uncontinuous_num "
                          << uncontinuous_num << " valid_group_size " << valid_group_size << " group_size " << group_size;
                if (valid_size < 1)
                    return -4;
                if (valid_group_size < 1)
                    return -5;
                if (valid_group_size < group_size / 2)
                    return -6;
                // if (valid_size > int(cloud_size * 0.5))
                //     return 1;
                if (uncontinuous_num < options.closure_check_uncontinuous_min_num)
                    return -7;
                return 2;
            }

            std::vector<Candidate2D> FastCorrelativeScanMatcher2D::ComputeLowestResolutionCandidates(
                const std::vector<DiscreteScan2D> &discrete_scans, const SearchParameters &search_parameters) const
            {
                // 生成最低分辨率的所有候选解, 根据一定的步长在X轴和Y轴上取若干个可能的位姿，然后返回所有的位姿
                std::vector<Candidate2D> lowest_resolution_candidates = GenerateLowestResolutionCandidates(search_parameters);
                // 遍历每个可能的位姿，以该位姿计算每个点的栅格索引，统计概率
                ScoreCandidates(precomputation_grid_stack_->Get(precomputation_grid_stack_->max_depth()), discrete_scans, search_parameters,
                                &lowest_resolution_candidates);
                return lowest_resolution_candidates;
            }

            std::vector<Candidate2D> FastCorrelativeScanMatcher2D::GenerateLowestResolutionCandidates(
                const SearchParameters &search_parameters) const
            {
                int curr_depth = precomputation_grid_stack_->max_depth();
                const int linear_step_size = 1 << precomputation_grid_stack_->max_depth();
                int num_candidates = 0;
                for (int scan_index = 0; scan_index != search_parameters.num_scans; ++scan_index)
                {
                    // x轴的偏移次数
                    const int num_lowest_resolution_linear_x_candidates =
                        (search_parameters.linear_bounds[scan_index].max_x - search_parameters.linear_bounds[scan_index].min_x +
                         linear_step_size) /
                        linear_step_size;
                    // y轴的偏移次数
                    const int num_lowest_resolution_linear_y_candidates =
                        (search_parameters.linear_bounds[scan_index].max_y - search_parameters.linear_bounds[scan_index].min_y +
                         linear_step_size) /
                        linear_step_size;
                    num_candidates += num_lowest_resolution_linear_x_candidates * num_lowest_resolution_linear_y_candidates;
                }
                // 总共有num_candidates个可能的位姿
                std::vector<Candidate2D> candidates;
                candidates.reserve(num_candidates);
                for (int scan_index = 0; scan_index != search_parameters.num_scans; ++scan_index)
                {
                    for (int x_index_offset = search_parameters.linear_bounds[scan_index].min_x;
                         x_index_offset <= search_parameters.linear_bounds[scan_index].max_x; x_index_offset += linear_step_size)
                    {
                        for (int y_index_offset = search_parameters.linear_bounds[scan_index].min_y;
                             y_index_offset <= search_parameters.linear_bounds[scan_index].max_y; y_index_offset += linear_step_size)
                        {
                            candidates.emplace_back(scan_index, x_index_offset, y_index_offset, search_parameters, curr_depth);
                        }
                    }
                }
                CHECK_EQ(candidates.size(), num_candidates);
                return candidates;
            }

            void FastCorrelativeScanMatcher2D::ScoreCandidates(const PrecomputationGrid2D &precomputation_grid,
                                                               const std::vector<DiscreteScan2D> &discrete_scans,
                                                               const SearchParameters &search_parameters,
                                                               std::vector<Candidate2D> *const candidates) const
            {
                for (Candidate2D &candidate : *candidates)
                {
                    int sum = 0;
                    for (const Eigen::Array2i &xy_index : discrete_scans[candidate.scan_index])
                    {
                        const Eigen::Array2i proposed_xy_index(xy_index.x() + candidate.x_index_offset,
                                                               xy_index.y() + candidate.y_index_offset);
                        sum += precomputation_grid.GetValue(proposed_xy_index);
                    }
                    candidate.score = precomputation_grid.ToScore(sum / static_cast<float>(discrete_scans[candidate.scan_index].size()));
                }
                // 从高到低排序
                std::sort(candidates->begin(), candidates->end(), std::greater<Candidate2D>());
            }

            Candidate2D FastCorrelativeScanMatcher2D::BranchAndBound(const std::vector<DiscreteScan2D> &discrete_scans,
                                                                     const SearchParameters &search_parameters,
                                                                     const std::vector<Candidate2D> &candidates, const int candidate_depth,
                                                                     float min_score) const
            {
                if (candidate_depth == 0)
                {
                    // Return the best candidate.
                    return *candidates.begin();
                }

                Candidate2D best_high_resolution_candidate(0, 0, 0, search_parameters);
                best_high_resolution_candidate.score = min_score;
                for (const Candidate2D &candidate : candidates)
                {
                    if (candidate.score <= min_score)
                    {
                        break;
                    }
                    std::vector<Candidate2D> higher_resolution_candidates;
                    int curr_depth = candidate_depth - 1;
                    const int half_width = 1 << (candidate_depth - 1);
                    for (int x_offset : {0, half_width})
                    {
                        if (candidate.x_index_offset + x_offset > search_parameters.linear_bounds[candidate.scan_index].max_x)
                        {
                            break;
                        }
                        for (int y_offset : {0, half_width})
                        {
                            if (candidate.y_index_offset + y_offset > search_parameters.linear_bounds[candidate.scan_index].max_y)
                            {
                                break;
                            }
                            higher_resolution_candidates.emplace_back(candidate.scan_index, candidate.x_index_offset + x_offset,
                                                                      candidate.y_index_offset + y_offset, search_parameters, curr_depth);
                        }
                    }
                    ScoreCandidates(precomputation_grid_stack_->Get(candidate_depth - 1), discrete_scans, search_parameters,
                                    &higher_resolution_candidates);
                    best_high_resolution_candidate = std::max(
                        best_high_resolution_candidate, BranchAndBound(discrete_scans, search_parameters, higher_resolution_candidates,
                                                                       candidate_depth - 1, best_high_resolution_candidate.score));
                }
                return best_high_resolution_candidate;
            }

        } // namespace scan_matching
    }     // namespace mapping
} // namespace cartographer
